/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
#include "BBSlammerWrapper.h"

BBSlammerWrapper::BBSlammerWrapper(){
	initVariables(0.06, 45.0, ///Map 
								0.4,0.4,15.0*M_PI/180.0, ///Search 
								25.0, 0.02, 0.5, ///Fdist, minspan, pth 
								0.3,6.0*M_PI/180.0, ///Odo 
								0.9,12.0*M_PI/180.0,25.0, 10.0);///Map
	
	slammer = new CBetBdtLocalizer(paramMapSize,paramMapResolution);
	slammer->tuneParameters(paramFdist, paramTh);
	slammer->setMinSpan(paramMinSpan);
}

BBSlammerWrapper::BBSlammerWrapper(float pMapResolution, float pMapSize,
																	 float pSearchX, float pSearchY, float pSearchA,
																	 float pFdist, float pMinSpan, float pTh,
																	 float pOdoCorrectDistance, float pOdoCorrectHeading,
																	 float pMapUpdateDistance, float pMapUpdateHeading, float pMaxScannerRange, 
																	 float pDistToEdge){
	initVariables(pMapResolution, pMapSize,
								pSearchX, pSearchY, pSearchA,
								pFdist, pMinSpan, pTh,
								pOdoCorrectDistance, pOdoCorrectHeading,
								pMapUpdateDistance, pMapUpdateHeading, pMaxScannerRange, pDistToEdge);///Map
	
	slammer = new CBetBdtLocalizer(paramMapSize,paramMapResolution);
	slammer->tuneParameters(paramFdist, paramTh);
	slammer->setMinSpan(paramMinSpan);
	
}

BBSlammerWrapper::BBSlammerWrapper(gim::CSettings &settings){
	initVariables(0.06, 45.0, ///Map 
								0.4,0.4,15.0*M_PI/180.0, ///Search 
								25.0, 0.02, 0.5, ///Fdist, minspan, pth 
								0.3,6.0*M_PI/180.0, ///Odo 
								0.9,12.0*M_PI/180.0,25.0, 10.0);///Map
	
	settings.GetValue("Mapper.grid_map_size", paramMapSize);
	settings.GetValue("Mapper.resolution",paramMapResolution);
	settings.GetValue("Mapper.fdist",paramFdist);
	settings.GetValue("Mapper.distth",paramTh);
	settings.GetValue("Mapper.minspan",paramMinSpan);
	settings.GetValue("Mapper.toEdge", paramToEdge);
	settings.GetValue("Mapper.searchX",paramSearchX);
	settings.GetValue("Mapper.searchY",paramSearchY);
	settings.GetValue("Mapper.searchA",paramSearchA);
	settings.GetValue("Mapper.OdoCorrectDistance",paramOdoCorrectDistance);
	settings.GetValue("Mapper.OdoCorrectHeading",paramOdoCorrectHeading);
	settings.GetValue("Mapper.MapUpdateDistance",paramMapUpdateDistance);
	settings.GetValue("Mapper.MapUpdateHeading",paramMapUpdateHeading);
	settings.GetValue("Mapper.MaxScannerRange",paramMaxScannerRange);
	
	settings.GetValue("Mapper.SameValueRatio",paramSameValueRatio);
	settings.GetValue("Mapper.NewValueRatio",paramNewValueRatio);
	settings.GetValue("Mapper.CheckMeasurement",paramCheckMeasurement);

	settings.GetValue("Mapper.UseInitialPoseFromLogfile",paramUseInitialPoseFromLogfile);
	
	slammer = new CBetBdtLocalizer(paramMapSize,paramMapResolution);
	slammer->tuneParameters(paramFdist, paramTh);
	slammer->setMinSpan(paramMinSpan);
}





bool BBSlammerWrapper::updateSimple(Grid::TScan &scan, Grid::pose pos){
	CMatchStorage matches(5);
	return(updateMeasurement(scan, pos, matches, false));
}


void BBSlammerWrapper::initVariables(float pMapResolution, float pMapSize,
												 float pSearchX, float pSearchY, float pSearchA,
												 float pFdist, float pMinSpan, float pTh,
												 float pOdoCorrectDistance, float pOdoCorrectHeading,
												 float pMapUpdateDistance, float pMapUpdateHeading, float pMaxScannerRange,
												 float pDistToEdge)
{
	
	paramSameValueRatio=0.9;		/// The measurement will be put into map if the number of "occupied" cells is less than the ratio
	paramNewValueRatio=0.1;					
	paramCheckMeasurement = 1; ///< use map to check if we need to update it
	paramMapResolution = pMapResolution;
	paramMapSize=pMapSize;
	///Search Volume
	paramSearchX=pSearchX; ///< Search area (X-coordinate)	
	paramSearchY=pSearchY; ///< Search area (Y-coordnate)
	paramSearchA=pSearchA; ///< Search area (Heading)
	///Tuning parameters
	paramFdist=pFdist; /// diastance that defines the Heading "span" 
	paramMinSpan=pMinSpan; /// < the minimum resolution of search
	paramTh=pTh; /// The threshold for maximum distance that effects the weight 
	///Update parameters
	paramOdoCorrectDistance=pOdoCorrectDistance; ///The distance between odometry measurements that will be corrected
	paramOdoCorrectHeading=pOdoCorrectHeading;  ///The change in heading that will be corrected 
	paramMapUpdateDistance=pMapUpdateDistance;  ///The distance that is used for map updates
	paramMapUpdateHeading=pMapUpdateHeading;   ///And the same for heading
	paramMaxScannerRange = pMaxScannerRange;
	paramToEdge = pDistToEdge;
	
	paramUseInitialPoseFromLogfile = 0;
	
	paramSetInitialPoseTo=0;
	paramInitPoseX=0;
	paramInitPoseY=0;
	paramInitPoseA=0;
	
	
}	
/**
* Sets initial pose from outside ... call before starting the estimation
*/
void BBSlammerWrapper::setInitialPose(float x, float y, float a){
	paramSetInitialPoseTo=1;
	paramInitPoseX=x;
	paramInitPoseY=y;
	paramInitPoseA=a;
}

bool BBSlammerWrapper::updateMeasurement(Grid::TScan &scan, Grid::pose pos, CMatchStorage matches, bool usePreviousLocalMap){
	CEnvironmentMeasurement2D meas;	
	meas.frame.setError(paramSearchX,paramSearchY,paramSearchA);
	bool updated = false;
	float distToEdge = paramToEdge;
	TMeasurementPoint p;
	tmp_scan.clear();
	meas.clear();
	///Filter measurement and save is to the localmap
	for(int i=0;i<scan.N;i++){
		if(scan.r[i]<paramMaxScannerRange && scan.r[i]>0){
			p.x = scan.r[i]*cos(scan.a[i]);
			p.y = scan.r[i]*sin(scan.a[i]);
			meas.addPoint(p);
		}else{
			scan.r[i] = 0;
		}
		tmp_scan.push_back(scan.r[i]);
	}
	scans.push_back(tmp_scan);
	
	///If this is the first round
	if(all_poses.size()==0){
		if(paramUseInitialPoseFromLogfile){
			slammer->setMeasurementToMap(scan,pos.x,pos.y,pos.a,0.999);
			init_est = pos;
			fprintf(stderr,"\nInit Pose from LOG = (%.2f , %.2f, %.2f)\n", init_est.x, init_est.y,init_est.a);
		}else if(paramSetInitialPoseTo){
			init_est.set(paramInitPoseX,paramInitPoseY,paramInitPoseA);
			slammer->setMeasurementToMap(scan,paramInitPoseX,paramInitPoseY,paramInitPoseA,0.999);	
			fprintf(stderr,"\nInit Pose from SETTINGS = (%.2f , %.2f, %.2f)\n", init_est.x, init_est.y,init_est.a);
		}else{
			slammer->setMeasurementToMap(scan,0,0,0,0.999);
		}
		current = pos;
		last=current;
		all_poses.push_back(init_est);
		poses.push_back(init_est); ///Save pose for this scan
		
		return true;
	}
	
	current = pos;
	diff.setToDifferentialPose(current,last);
	init_est = init_est.integrateDifferential(diff);
	last=current;
	sinceLastOdoCorrection = sinceLastOdoCorrection.integrateDifferential(diff);
	sinceLastMapCorrection = sinceLastMapCorrection.integrateDifferential(diff);
	sinceLastOdoCorrection.toPI();
	sinceLastMapCorrection.toPI();
	
	///Update only if we have moved
	if(sinceLastOdoCorrection.getDist() > paramOdoCorrectDistance || 
		fabs(sinceLastOdoCorrection.a)>paramMapUpdateHeading ||
		sinceLastMapCorrection.getDist() > paramMapUpdateDistance || 
		fabs(sinceLastMapCorrection.a) > paramMapUpdateHeading)
	{
		meas.frame.setFrame(init_est.x,init_est.y,init_est.a);
		if(slammer->localize(meas,matches)){
			matches.sort();
			fprintf(stderr,"dist = %.2f A=%.2f", sinceLastOdoCorrection.getDist(),fabs(sinceLastOdoCorrection.a)*180/M_PI);
			init_est.set(matches.storage[0].x,matches.storage[0].y,matches.storage[0].a);
			sinceLastOdoCorrection.set(0,0,0);
		}else{
			fprintf(stderr,"Localization Failed - using initial estimate!!!!\n");
		}
	}
	
	pose_now = reference_pose.integrateDifferential(init_est); ///< Current pose
	all_poses.push_back(pose_now);
	
	///Do we meet the map update criteria 
	if(sinceLastMapCorrection.getDist() > paramMapUpdateDistance || 
		fabs(sinceLastMapCorrection.a) > paramMapUpdateHeading)
	{
		int Nc = 0; ///< conflicting
		int Ns = 0; ///< the same
		int Nu = 0; ///< undefined
		
		if(paramCheckMeasurement){
			for(int i=0;i<scan.N;i++){
				Grid::TCoord2D mpix( (scan.r[i]*cos(scan.a[i]+init_est.a)+init_est.x),  (scan.r[i]*sin(scan.a[i]+init_est.a)+init_est.y));
				float v;
				
				if(slammer->grid->getValue(mpix,v)){
					if( fabs(v-0.5)<0.05 ) Nu++;
					if( v>0.6 ) Ns++;
					if( v<0.4 ) Nc++;
				}
			}
			fprintf(stderr,"\t(Ns, Nu, Nc) = (%d, %d, %d)\t",Ns, Nu, Nc);
			float sameval = ((float)Ns)/((float)(Ns+Nu+Nc));
			float newval = ((float)Nu)/((float)(Ns+Nu+Nc));		
			if(sameval<paramSameValueRatio || newval>paramNewValueRatio){
				slammer->setMeasurementToMap(scan,init_est.x,init_est.y,init_est.a,0.98);
				sinceLastMapCorrection.set(0,0,0);
				fprintf(stderr," - Map Update - ");
			}
		}else{
			slammer->setMeasurementToMap(scan,init_est.x,init_est.y,init_est.a,0.98);
			sinceLastMapCorrection.set(0,0,0);
			fprintf(stderr," - Map Update (N=%d) - ", scan.N);
		}
	}
	
	////////////////////////////////////////////////////////////////////////////
	/// Resetting the map
	////////////////////////////////////////////////////////////////////////////
	float toEdge=(paramMapSize/2.0)-init_est.getDist();
	if(toEdge < distToEdge){
		slammer->grid->setGridToValue(0.5); ///< Clear map
		reference_pose = reference_pose.integrateDifferential(init_est); ///< Current pose
		init_est.set(0,0,0);
		
		if(usePreviousLocalMap){
			fprintf(stderr,"SORRY PREVIOUS MAP INSERTION IS CURRENTLY NOT IMPLEMENTED!!\n");
		}
		slammer->setMeasurementToMap(scan,0,0,0,0.999);
		
		scans.clear();
		poses.clear();
	}
	
	
	
	return true;
}